#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
基于路径规划成功的导航测试节点
功能：
1. 通过检测全局路径规划是否成功来判断导航状态
2. 每2秒自动发送新目标点
3. 精确记录规划时间和路径质量
4. 生成详细测试报告
"""

import rospy
import random
import time
import math
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseActionGoal
from std_msgs.msg import Float32
from tabulate import tabulate
import csv
from datetime import datetime

class PathPlanningTester:
    def __init__(self):
        rospy.init_node('path_planning_tester')
        
        # 参数配置
        self.test_area = {
            'x_min': -4.0, 'x_max': 4.0,
            'y_min': -4.0, 'y_max': 4.0
        }
        self.total_attempts = 100
        self.min_distance = 1.0
        self.interval = 2.0  # 发送间隔(秒)
        self.planning_timeout = 5.0  # 规划超时时间(秒)
        
        # 发布/订阅
        self.goal_pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10)
        self.planning_pub = rospy.Publisher('/slam/planning_time', Float32, queue_size=10)
        self.global_path_sub = rospy.Subscriber('/move_base/NavfnROS/plan', Path, self.path_callback)
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        
        # 状态变量
        self.current_attempt = None
        self.attempt_data = []
        self.attempt_counter = 0
        self.last_send_time = 0
        self.last_path_time = 0
        self.current_position = (0, 0)
        
        # 定时器
        self.timer = rospy.Timer(rospy.Duration(self.interval), self.timer_callback)
        rospy.loginfo("Path planning tester initialized. Total attempts: {}".format(self.total_attempts))

    def timer_callback(self, event):
        """定时发送新目标点"""
        if self.attempt_counter >= self.total_attempts:
            self.timer.shutdown()
            rospy.loginfo("Reached maximum attempt count")
            return
            
        current_time = time.time()
        if current_time - self.last_send_time >= self.interval:
            self.send_new_goal()
            self.last_send_time = current_time

    def send_new_goal(self):
        """发送新目标点"""
        # 生成有效目标位置
        goal_pos = self.generate_valid_goal()
                
        # 准备目标消息
        goal_msg = MoveBaseActionGoal()
        goal_msg.goal_id.stamp = rospy.Time.now()
        goal_msg.goal_id.id = "attempt_{:03d}".format(self.attempt_counter)
        goal_msg.goal.target_pose.header.frame_id = "map"
        goal_msg.goal.target_pose.header.stamp = rospy.Time.now()
        goal_msg.goal.target_pose.pose.position.x = goal_pos[0]
        goal_msg.goal.target_pose.pose.position.y = goal_pos[1]
        goal_msg.goal.target_pose.pose.orientation.w = 1.0
        
        # 记录尝试信息
        self.current_attempt = {
            'id': goal_msg.goal_id.id,
            'start_time': time.time(),
            'goal_pos': goal_pos,
            'start_pos': self.current_position,
            'path_received': False,
            'planning_time': None,
            'path_length': None,
            'success': None
        }
        
        self.goal_pub.publish(goal_msg)
        self.attempt_counter += 1
        rospy.loginfo("Sent {} to ({:.2f}, {:.2f})".format(
            self.current_attempt['id'], goal_pos[0], goal_pos[1]))
        
        # 设置规划超时检查
        rospy.Timer(rospy.Duration(self.planning_timeout), 
                   self.check_planning_timeout, 
                   oneshot=True)

    def generate_valid_goal(self):
        """生成符合距离要求的目标点"""
        for _ in range(100):  # 最大尝试次数
            goal_pos = (
                random.uniform(self.test_area['x_min'], self.test_area['x_max']),
                random.uniform(self.test_area['y_min'], self.test_area['y_max'])
            )
            if self.calculate_distance(self.current_position, goal_pos) >= self.min_distance:
                return goal_pos
        return (self.current_position[0]+self.min_distance, 
                self.current_position[1]+self.min_distance)  # 保底返回

    def path_callback(self, msg):
        """全局路径回调（判断规划是否成功）"""
        if not self.current_attempt or self.current_attempt['path_received']:
            return
            
        # 检查路径有效性
        if len(msg.poses) > 1:  # 有效路径应至少包含2个点
            planning_time = time.time() - self.current_attempt['start_time']
            path_length = self.calculate_path_length(msg.poses)
            
            self.current_attempt.update({
                'path_received': True,
                'planning_time': planning_time,
                'path_length': path_length,
                'success': True
            })
            
            self.planning_pub.publish(Float32(planning_time))
            rospy.loginfo("{} planning SUCCESS in {:.2f}s (path length: {:.2f}m)".format(
                self.current_attempt['id'], planning_time, path_length))
            
            # 记录并准备下一次尝试
            self.record_attempt()
        else:
            rospy.logwarn("{} received empty path!".format(self.current_attempt['id']))

    def check_planning_timeout(self, event):
        """规划超时检查"""
        if not self.current_attempt or self.current_attempt['path_received']:
            return
            
        self.current_attempt.update({
            'success': False,
            'planning_time': self.planning_timeout,
            'path_length': 0
        })
        rospy.logwarn("{} planning TIMEOUT (>{:.1f}s)".format(
            self.current_attempt['id'], self.planning_timeout))
        
        self.record_attempt()

    def record_attempt(self):
        """记录完成的尝试"""
        if not self.current_attempt:
            return
            
        self.attempt_data.append(self.current_attempt)
        
        # 实时统计
        success_rate = self.calculate_success_rate()
        rospy.loginfo("Current success rate: {:.2f}% ({}/{})".format(
            success_rate, 
            sum(1 for a in self.attempt_data if a['success']), 
            len(self.attempt_data)))
        
        # 重置当前尝试
        self.current_attempt = None
        
        # 测试完成检查
        if len(self.attempt_data) >= self.total_attempts:
            self.generate_report()
            rospy.signal_shutdown("Test completed")

    def odom_callback(self, msg):
        """更新当前位置"""
        self.current_position = (
            msg.pose.pose.position.x, 
            msg.pose.pose.position.y
        )

    def calculate_distance(self, pos1, pos2):
        """计算两点距离"""
        return math.hypot(pos1[0]-pos2[0], pos1[1]-pos2[1])

    def calculate_path_length(self, poses):
        """计算路径长度"""
        length = 0.0
        for i in range(1, len(poses)):
            p1 = poses[i-1].pose.position
            p2 = poses[i].pose.position
            length += math.hypot(p1.x-p2.x, p1.y-p2.y)
        return length

    def calculate_success_rate(self):
        """计算当前成功率"""
        if not self.attempt_data:
            return 0.0
        successes = sum(1 for a in self.attempt_data if a['success'])
        return (successes / len(self.attempt_data)) * 100

    def generate_report(self):
        """生成最终测试报告"""
        if not self.attempt_data:
            rospy.logerr("No attempt data to generate report!")
            return
        
        # 统计数据
        successes = [a for a in self.attempt_data if a['success']]
        failures = [a for a in self.attempt_data if not a['success']]
        
        stats = {
            'total': len(self.attempt_data),
            'success': len(successes),
            'failure': len(failures),
            'success_rate': self.calculate_success_rate(),
            'avg_plan_time': sum(a['planning_time'] for a in successes) / len(successes) if successes else 0,
            'avg_path_len': sum(a['path_length'] for a in successes) / len(successes) if successes else 0,
        }
        
        # 生成报告
        report = [
            "\n" + "="*70,
            "          PATH PLANNING PERFORMANCE REPORT",
            "="*70,
            f"\nTest Configuration:",
            f"- Area: X[{self.test_area['x_min']:.1f} to {self.test_area['x_max']:.1f}m]",
            f"       Y[{self.test_area['y_min']:.1f} to {self.test_area['y_max']:.1f}m]",
            f"- Attempts: {stats['total']} | Min Distance: {self.min_distance}m",
            f"- Planning Timeout: {self.planning_timeout}s",
            f"\nResults:",
            f"- Planning Success Rate: {stats['success_rate']:.2f}% ({stats['success']}/{stats['total']})",
            f"- Avg Planning Time (successful): {stats['avg_plan_time']:.2f}s",
            f"- Avg Path Length: {stats['avg_path_len']:.2f}m",
            f"- Failure Reasons:",
            f"  - Timeout: {len(failures)} cases",
            "="*70
        ]
        
        # 打印并保存报告
        print("\n".join(report))
        self.save_to_csv()

    def save_to_csv(self):
        """保存测试数据到CSV"""
        filename = "path_planning_test_{}.csv".format(datetime.now().strftime("%Y%m%d_%H%M%S"))
        
        with open(filename, 'w') as f:
            writer = csv.writer(f)
            writer.writerow([
                'ID', 'Success', 'Planning_Time(s)', 'Path_Length(m)',
                'Start_X', 'Start_Y', 'Goal_X', 'Goal_Y'
            ])
            
            for attempt in sorted(self.attempt_data, key=lambda x: x['id']):
                writer.writerow([
                    attempt['id'],
                    attempt['success'],
                    round(attempt['planning_time'], 3),
                    round(attempt['path_length'], 2),
                    round(attempt['start_pos'][0], 3),
                    round(attempt['start_pos'][1], 3),
                    round(attempt['goal_pos'][0], 3),
                    round(attempt['goal_pos'][1], 3)
                ])
        
        rospy.loginfo("Test data saved to {}".format(filename))

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    try:
        tester = PathPlanningTester()
        tester.run()
    except rospy.ROSInterruptException:
        pass